/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Willow Garage, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
* Author: Dave Hershberger
*********************************************************************/
#include <gtest/gtest.h>
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_listener.h>

using namespace costmap_2d;

tf2_ros::TransformListener *tfl_;
tf2_ros::Buffer *tf_;

TEST(Costmap2DROS, unpadded_footprint_from_string_param)
{
    Costmap2DROS cm("unpadded/string", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    EXPECT_EQ(3, footprint.size());

    EXPECT_EQ(1.0f, footprint[0].x);
    EXPECT_EQ(1.0f, footprint[0].y);
    EXPECT_EQ(0.0f, footprint[0].z);

    EXPECT_EQ(-1.0f, footprint[1].x);
    EXPECT_EQ(1.0f, footprint[1].y);
    EXPECT_EQ(0.0f, footprint[1].z);

    EXPECT_EQ(-1.0f, footprint[2].x);
    EXPECT_EQ(-1.0f, footprint[2].y);
    EXPECT_EQ(0.0f, footprint[2].z);
}

TEST(Costmap2DROS, padded_footprint_from_string_param)
{
    Costmap2DROS cm("padded/string", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    EXPECT_EQ(3, footprint.size());

    EXPECT_EQ(1.5f, footprint[0].x);
    EXPECT_EQ(1.5f, footprint[0].y);
    EXPECT_EQ(0.0f, footprint[0].z);

    EXPECT_EQ(-1.5f, footprint[1].x);
    EXPECT_EQ(1.5f, footprint[1].y);
    EXPECT_EQ(0.0f, footprint[1].z);

    EXPECT_EQ(-1.5f, footprint[2].x);
    EXPECT_EQ(-1.5f, footprint[2].y);
    EXPECT_EQ(0.0f, footprint[2].z);
}

TEST(Costmap2DROS, radius_param)
{
    Costmap2DROS cm("radius/sub", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    // Circular robot has 16-point footprint auto-generated.
    EXPECT_EQ(16, footprint.size());

    // Check the first point
    EXPECT_EQ(10.0f, footprint[0].x);
    EXPECT_EQ(0.0f, footprint[0].y);
    EXPECT_EQ(0.0f, footprint[0].z);

    // Check the 4th point, which should be 90 degrees around the circle from the first.
    EXPECT_NEAR(0.0f, footprint[4].x, 0.0001);
    EXPECT_NEAR(10.0f, footprint[4].y, 0.0001);
    EXPECT_EQ(0.0f, footprint[4].z);
}

TEST(Costmap2DROS, footprint_from_xmlrpc_param)
{
    Costmap2DROS cm("xmlrpc", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    EXPECT_EQ(4, footprint.size());

    EXPECT_EQ(0.1f, footprint[0].x);
    EXPECT_EQ(0.1f, footprint[0].y);
    EXPECT_EQ(0.0f, footprint[0].z);

    EXPECT_EQ(-0.1f, footprint[1].x);
    EXPECT_EQ(0.1f, footprint[1].y);
    EXPECT_EQ(0.0f, footprint[1].z);

    EXPECT_EQ(-0.1f, footprint[2].x);
    EXPECT_EQ(-0.1f, footprint[2].y);
    EXPECT_EQ(0.0f, footprint[2].z);

    EXPECT_EQ(0.1f, footprint[3].x);
    EXPECT_EQ(-0.1f, footprint[3].y);
    EXPECT_EQ(0.0f, footprint[3].z);
}

TEST(Costmap2DROS, footprint_from_same_level_param)
{
    Costmap2DROS cm("same_level", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    EXPECT_EQ(3, footprint.size());

    EXPECT_EQ(1.0f, footprint[0].x);
    EXPECT_EQ(2.0f, footprint[0].y);
    EXPECT_EQ(0.0f, footprint[0].z);

    EXPECT_EQ(3.0f, footprint[1].x);
    EXPECT_EQ(4.0f, footprint[1].y);
    EXPECT_EQ(0.0f, footprint[1].z);

    EXPECT_EQ(5.0f, footprint[2].x);
    EXPECT_EQ(6.0f, footprint[2].y);
    EXPECT_EQ(0.0f, footprint[2].z);
}

TEST(Costmap2DROS, footprint_from_xmlrpc_param_failure)
{
    ASSERT_ANY_THROW(Costmap2DROS cm("xmlrpc_fail", *tf_));
}

TEST(Costmap2DROS, footprint_empty)
{
    Costmap2DROS cm("empty", *tf_);
    std::vector<geometry_msgs::Point> footprint = cm.getRobotFootprint();
    // With no specification of footprint or radius, defaults to 0.46 meter radius plus 0.01 meter padding.
    EXPECT_EQ(16, footprint.size());
    EXPECT_NEAR(0.47f, footprint[0].x, 0.0001);
    EXPECT_NEAR(0.0f, footprint[0].y, 0.0001);
    EXPECT_EQ(0.0f, footprint[0].z);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "footprint_tests_node");
    tf_ = new tf2_ros::Buffer(ros::Duration(10));
    tfl_ = new tf2_ros::TransformListener(*tf_);
    // This empty transform is added to satisfy the constructor of Costmap2DROS,
    // which waits for the transform from map to base_link to become available.
    geometry_msgs::TransformStamped base_rel_map;
    base_rel_map.transform = tf2::toMsg(tf2::Transform::getIdentity());
    base_rel_map.child_frame_id = "base_link";
    base_rel_map.header.frame_id = "map";
    base_rel_map.header.stamp = ros::Time::now();
    tf_->setTransform(base_rel_map, "footprint_tests");
    testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}
